using BepuUtilities;
using DemoRenderer;
using BepuPhysics;
using BepuPhysics.Collidables;
using System;
using DemoContentLoader;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif

namespace Demos.Demos
{
    public class CompoundTestDemo : Demo
    {
        public unsafe override void Initialize(ContentArchive content, Camera camera)
        {
            camera.Position = new Vector3(-13f, 6, -13f);
            camera.Yaw = MathHelper.Pi * 3f / 4;
            camera.Pitch = MathHelper.Pi * 0.05f;

            // PositionFirstTimestpper是最简单的时步模式,但由于它在帧开始时将速度积分到位置中,因此直接修改了时间步长之外的速度
            // 将在碰撞检测或解算器有机会介入之前集成。这在这个演示中很好。其他内置选项包括PositionLastTimestpper和SubsteppingTimestpper。
            // 请注意,timeSteper还具有回调,您可以使用这些回调在处理阶段之间执行逻辑,如BeforeCollisionDetection。
            Simulation = Simulation.Create(BufferPool, new DemoNarrowPhaseCallbacks(), new DemoPoseIntegratorCallbacks(new Vector3(0, -10f, 0)), new PositionFirstTimestepper());

            using (var compoundBuilder = new CompoundBuilder(BufferPool, Simulation.Shapes, 8))
            {
                {
                    // 请注意,在BepuPhysical v2中,在构造形状时没有"重新居中"。您为孩子传递的姿势正好是复合体将使用的姿势,
                    // 即使"真正的"质心不在当地原点。
                    // 相反,如果需要重新定心,则应提前执行。CompoundBuilder可以在这方面提供帮助。
                    // 我们将使用远离原点的形状构造此复合,然后使用使子项重新居中并输出计算中心的CompoundBuilder重载。
                    var capsuleChildShape = new Capsule(0.5f, 0.5f);
                    var capsuleLocalPose = new RigidPose { Position = new Vector3(-0.5f, 4, 4), Orientation = Quaternion.Identity };
                    var boxChildShape = new Box(0.5f, 1f, 1.5f);
                    var boxLocalPose = new RigidPose { Position = new Vector3(0.5f, 4, 4), Orientation = Quaternion.Identity };

                    // 在处理演示时,将处理用于最终复合形状的缓冲池中的所有分配。不必担心这些演示中的泄密。
                    compoundBuilder.Add(capsuleChildShape, capsuleLocalPose, 1);
                    compoundBuilder.Add(boxChildShape, boxLocalPose, 1);
                    compoundBuilder.BuildDynamicCompound(out var compoundChildren, out var compoundInertia, out var compoundCenter);
                    compoundBuilder.Reset();
                    Simulation.Bodies.Add(BodyDescription.CreateDynamic(compoundCenter, compoundInertia, new CollidableDescription(Simulation.Shapes.Add(new Compound(compoundChildren)), 0.1f), new BodyActivityDescription(0.01f)));
                }

                // 构建一堆球面网格,以强调凸形情况下的流形简化启发式。
                {
                    var gridShape = new Sphere(0.5f);
                    const float gridSpacing = 1.5f;
                    const int gridWidth = 3;
                    var gridShapeIndex = Simulation.Shapes.Add(gridShape);
                    gridShape.ComputeInertia(1, out var gridBoxInertia);
                    float localPoseOffset = -0.5f * gridSpacing * (gridWidth - 1);
                    for (int i = 0; i < gridWidth; ++i)
                    {
                        for (int j = 0; j < gridWidth; ++j)
                        {
                            compoundBuilder.Add(gridShapeIndex, new RigidPose(new Vector3(localPoseOffset, 0, localPoseOffset) + new Vector3(gridSpacing) * new Vector3(i, 0, j)), gridBoxInertia.InverseInertiaTensor, 1);
                        }
                    }
                    compoundBuilder.BuildDynamicCompound(out var gridChildren, out var gridInertia, out var center);
                    compoundBuilder.Reset();
                    var gridCompound = new Compound(gridChildren);
                    var bodyDescription = BodyDescription.CreateDynamic(RigidPose.Identity, gridInertia, new CollidableDescription(Simulation.Shapes.Add(gridCompound), 0.1f), new BodyActivityDescription(0.01f));
                    for (int i = 0; i < 4; ++i)
                    {
                        bodyDescription.Pose.Position = new Vector3(0, 2 + i * 3, 0);
                        Simulation.Bodies.Add(bodyDescription);
                    }
                }

                // 构建一个表,并将其用于几个不同的测试。
                {
                    var legShape = new Box(0.2f, 1, 0.2f);
                    legShape.ComputeInertia(1f, out var legInverseInertia);
                    var legShapeIndex = Simulation.Shapes.Add(legShape);
                    var legPose0 = new RigidPose { Position = new Vector3(-1.5f, 0, -1.5f), Orientation = Quaternion.Identity };
                    var legPose1 = new RigidPose { Position = new Vector3(-1.5f, 0, 1.5f), Orientation = Quaternion.Identity };
                    var legPose2 = new RigidPose { Position = new Vector3(1.5f, 0, -1.5f), Orientation = Quaternion.Identity };
                    var legPose3 = new RigidPose { Position = new Vector3(1.5f, 0, 1.5f), Orientation = Quaternion.Identity };
                    compoundBuilder.Add(legShapeIndex, legPose0, legInverseInertia.InverseInertiaTensor, 1);
                    compoundBuilder.Add(legShapeIndex, legPose1, legInverseInertia.InverseInertiaTensor, 1);
                    compoundBuilder.Add(legShapeIndex, legPose2, legInverseInertia.InverseInertiaTensor, 1);
                    compoundBuilder.Add(legShapeIndex, legPose3, legInverseInertia.InverseInertiaTensor, 1);
                    var tableTopPose = new RigidPose { Position = new Vector3(0, 0.6f, 0), Orientation = Quaternion.Identity };
                    var tableTopShape = new Box(3.2f, 0.2f, 3.2f);
                    compoundBuilder.Add(tableTopShape, tableTopPose, 3);

                    compoundBuilder.BuildDynamicCompound(out var tableChildren, out var tableInertia, out var tableCenter);
                    compoundBuilder.Reset();
                    var table = new Compound(tableChildren);
                    var tableDescription = new BodyDescription
                    {
                        Activity = new BodyActivityDescription { SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32 },
                        Collidable = new CollidableDescription
                        {
                            Shape = Simulation.Shapes.Add(table),
                            SpeculativeMargin = 0.1f,
                        },
                        LocalInertia = tableInertia,
                        Pose = new RigidPose { Orientation = Quaternion.Identity }
                    };

                    // 叠几张桌子。
                    {
                        for (int i = 0; i < 10; ++i)
                        {
                            tableDescription.Pose.Position = new Vector3(10, 3 + i * 1.4f, 10);
                            Simulation.Bodies.Add(tableDescription);
                        }
                    }
                    {
                        for (int k = 0; k < 5; ++k)
                        {
                            tableDescription.Pose.Position = new Vector3(64 + k * 3, 6 + k * 1.4f, 32);
                            Simulation.Bodies.Add(tableDescription);
                        }
                        // for(int i=0;i<10;++i)
                        // {
                        // for(int j=0;j<20;++j)
                        // {
                        // for(int k=0;k<10;++k)
                        // {
                        // tableDescription.Pose.Position=new Vector3(32+i*6,6+j*1.4f,16+k*6);
                        // Simulation.Bodies.Add(TableDescription);
                        // }
                        // }
                        // }
                    }

                    // 将一张桌子放在球体的顶部,以突出发散法线的非凸化。
                    {
                        tableDescription.Pose.Position = new Vector3(10, 6, 0);
                        Simulation.Bodies.Add(tableDescription);

                        var sphereShape = new Sphere(3);
                        var sphereIndex = Simulation.Shapes.Add(sphereShape);
                        var sphereDescription = new StaticDescription
                        {
                            Collidable = new CollidableDescription
                            {
                                Shape = sphereIndex,
                                SpeculativeMargin = 0.1f,
                            },
                            Pose = new RigidPose { Position = new Vector3(10, 2, 0), Orientation = Quaternion.Identity }
                        };
                        Simulation.Statics.Add(sphereDescription);
                    }

                    // 把另一张桌子放在地上,但上面有一个夹子一样的东西,可以产生相反的法线。
                    {
                        tableDescription.Pose.Position = new Vector3(10, 3, -10);
                        Simulation.Bodies.Add(tableDescription);

                        var clampPieceShape = new Box(2f, 0.1f, 0.3f);
                        clampPieceShape.ComputeInertia(1f, out var clampPieceInverseInertia);
                        var clampPieceShapeIndex = Simulation.Shapes.Add(clampPieceShape);
                        var clamp0 = new RigidPose { Position = new Vector3(0, -0.2f, -1.1f), Orientation = Quaternion.Identity };
                        var clamp1 = new RigidPose { Position = new Vector3(0, 0.2f, -1.1f), Orientation = Quaternion.Identity };
                        var clamp2 = new RigidPose { Position = new Vector3(0, -0.2f, 0), Orientation = Quaternion.Identity };
                        var clamp3 = new RigidPose { Position = new Vector3(0, 0.2f, 0), Orientation = Quaternion.Identity };
                        var clamp4 = new RigidPose { Position = new Vector3(0, -0.2f, 1.1f), Orientation = Quaternion.Identity };
                        var clamp5 = new RigidPose { Position = new Vector3(0, 0.2f, 1.1f), Orientation = Quaternion.Identity };
                        compoundBuilder.Add(clampPieceShapeIndex, clamp0, clampPieceInverseInertia.InverseInertiaTensor, 1);
                        compoundBuilder.Add(clampPieceShapeIndex, clamp1, clampPieceInverseInertia.InverseInertiaTensor, 1);
                        compoundBuilder.Add(clampPieceShapeIndex, clamp2, clampPieceInverseInertia.InverseInertiaTensor, 1);
                        compoundBuilder.Add(clampPieceShapeIndex, clamp3, clampPieceInverseInertia.InverseInertiaTensor, 1);
                        compoundBuilder.Add(clampPieceShapeIndex, clamp4, clampPieceInverseInertia.InverseInertiaTensor, 1);
                        compoundBuilder.Add(clampPieceShapeIndex, clamp5, clampPieceInverseInertia.InverseInertiaTensor, 1);

                        compoundBuilder.BuildDynamicCompound(out var clampChildren, out var clampInertia, out var clampCenter);
                        compoundBuilder.Reset();
                        var clamp = new Compound(clampChildren);
                        var clampDescription = new BodyDescription
                        {
                            Activity = new BodyActivityDescription { SleepThreshold = 0.01f, MinimumTimestepCountUnderThreshold = 32 },
                            Collidable = new CollidableDescription
                            {
                                Shape = Simulation.Shapes.Add(clamp),
                                SpeculativeMargin = 0.1f,
                            },
                            LocalInertia = clampInertia,
                            Pose = new RigidPose { Position = tableDescription.Pose.Position + new Vector3(2f, 0.3f, 0), Orientation = Quaternion.Identity }
                        };
                        Simulation.Bodies.Add(clampDescription);
                    }

                }

                // 创建一个树木加速的大型复合体。
                {
                    var random = new Random(5);
                    var treeCompoundBoxShape = new Box(0.5f, 1.5f, 1f);
                    var treeCompoundBoxShapeIndex = Simulation.Shapes.Add(treeCompoundBoxShape);
                    treeCompoundBoxShape.ComputeInertia(1, out var childInertia);
                    for (int i = 0; i < 128; ++i)
                    {
                        RigidPose localPose;
                        localPose.Position = new Vector3(12, 6, 12) * (0.5f * new Vector3((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble()) - Vector3.One);
                        float orientationLengthSquared;
                        do
                        {
                            localPose.Orientation = new Quaternion((float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble(), (float)random.NextDouble());
                            orientationLengthSquared = QuaternionEx.LengthSquared(ref localPose.Orientation);
                        }
                        while (orientationLengthSquared < 1e-9f);
                        QuaternionEx.Scale(localPose.Orientation, 1f / MathF.Sqrt(orientationLengthSquared), out localPose.Orientation);
                        // Quaternion.CreateFromAxisangle(new Vector3(1,0,0),MathF.PI,out localPose.Orientation);

                        compoundBuilder.Add(treeCompoundBoxShapeIndex, localPose, childInertia.InverseInertiaTensor, 1);
                    }
                    compoundBuilder.BuildDynamicCompound(out var children, out var inertia, out var center);
                    compoundBuilder.Reset();

                    var compound = new BigCompound(children, Simulation.Shapes, BufferPool);
                    // VAR化合物=新化合物(儿童);
                    var compoundIndex = Simulation.Shapes.Add(compound);
                    for (int i = 0; i < 8; ++i)
                    {
                        Simulation.Bodies.Add(BodyDescription.CreateDynamic(new Vector3(0, 4 + 5 * i, 32), inertia, new CollidableDescription(compoundIndex, 0.1f), new BodyActivityDescription(0.01f)));
                    }
                }
            }

            // 防止东西掉进无限的空虚。
            {
                var boxShape = new Box(256, 1, 256);
                var groundShapeIndex = Simulation.Shapes.Add(boxShape);
                var groundDescription = new StaticDescription
                {
                    Collidable = new CollidableDescription
                    {
                        Shape = groundShapeIndex,
                        SpeculativeMargin = 0.1f,
                    },
                    Pose = new RigidPose { Position = new Vector3(0, 0, 0), Orientation = Quaternion.Identity }
                };
                Simulation.Statics.Add(groundDescription);
            }
            const int planeWidth = 48;
            const int planeHeight = 48;
            DemoMeshHelper.CreateDeformedPlane(planeWidth, planeHeight,
                (int x, int y) =>
                        {
                            Vector2 offsetFromCenter = new Vector2(x - planeWidth / 2, y - planeHeight / 2);
                            return new Vector3(offsetFromCenter.X, MathF.Cos(x / 4f) * MathF.Sin(y / 4f) - 0.01f * offsetFromCenter.LengthSquared(), offsetFromCenter.Y);
                        }, new Vector3(2, 1, 2), BufferPool, out var planeMesh);
            Simulation.Statics.Add(new StaticDescription(new Vector3(64, 4, 32), QuaternionEx.CreateFromAxisAngle(new Vector3(0, 1, 0), MathF.PI / 2),
                new CollidableDescription(Simulation.Shapes.Add(planeMesh), 0.1f)));
        }
    }
}
